﻿using System;
using System.Collections.Generic;
using System.Text;
using AForge.Robotics.Lego;
using NKH.MindSqualls;
using System.Threading;

namespace NXTRobotController
{
    public enum LEGOAction
    {
        Tilt = 0,
        GrabPlus90 = 1,
        GrabMinus90 = 2,
        Grab180 = 3,
        TurnPlus90 = 4,
        TurnMinus90 = 5,
        Turn180 = 6
    }

    public class LEGORobotControl
    {
        private const string TILT_PROGRAM = "rubick.rxe";
        private const string GRABPLUS90_PROGRAM = "grabberPlus90.rxe";
        private const string GRABMINUS90_PROGRAM = "grabberMinus90.rxe";
        private const string GRAB180_PROGRAM = "grabber180.rxe";
        private const string TURNPLUS90_PROGRAM = "turnplus90.rxe";
        private const string TURNMINUS90_PROGRAM = "turnminus90.rxe";
        private const string TURN180_PROGRAM = "turn180.rxe";

        //NxtUsbConnection conn2;
        NxtUsbConnection conn;

        private byte port;

        /// <summary>
        /// Connect to the NXT Robot
        /// </summary>
        /// <param name="comPort">The COM port for the connection</param>
        /// <returns>Returns a boolean with the connection result</returns>
        public bool Connect(byte comPort)
        {
            bool retVal = true;
            if (IsConnected() || port != comPort)
            {
                conn = new NxtUsbConnection();
                try
                {
                    conn.Connect();
                    retVal = true;
                }
                catch
                {
                    retVal = false;
                }
            }
            return retVal;
        }

        public bool IsConnected()
        {
            return (conn != null && conn.IsConnected);
        }

        /// <summary>
        /// Disconnect the robot
        /// </summary>
        public void Disconnect()
        {
            if (conn != null && conn.IsConnected)
                conn.Disconnect();
        }

        public void DoInitializationMovement()
        {
            /*
            B-Vd-Am(L)
            Face L: Vd
            Face F: am
            Face R: Az
            Face B: b
            Face D: L
            Face U: Vm
             */
            DoAction(LEGOAction.Turn180);
            DoAction(LEGOAction.Tilt);
            DoAction(LEGOAction.TurnPlus90);
        }

        public void DoAction(LEGOAction action)
        {
            switch (action)
            {
                case LEGOAction.Tilt:
                    DoAction(TILT_PROGRAM);
                    break;
                case LEGOAction.Grab180:
                    DoAction(GRAB180_PROGRAM);
                    break;
                case LEGOAction.GrabMinus90:
                    DoAction(GRABMINUS90_PROGRAM);
                    break;
                case LEGOAction.GrabPlus90:
                    DoAction(GRABPLUS90_PROGRAM);
                    break;
                case LEGOAction.TurnPlus90:
                    DoAction(TURNPLUS90_PROGRAM);
                    break;
                case LEGOAction.TurnMinus90:
                    DoAction(TURNMINUS90_PROGRAM);
                    break;
                case LEGOAction.Turn180:
                    DoAction(TURN180_PROGRAM);
                    break;
                default:
                    break;
            }
        }

        public void DoAction(string actionName)
        {
            if (conn.IsConnected)
            {
                conn.StartProgram(actionName);
                WaitCompletion();
            }
        }

        private void WaitCompletion()
        {
            string t = "Teste";
            do
            {
                t = conn.GetCurrentProgramName();
            } while (!String.IsNullOrEmpty(t));
            Thread.Sleep(6000);
        }
    }
}
